//
// Created by Pulsar on 2020/5/16.
//

#include <rc_task/rcCVTask.h>
#include <iostream>
#include <base/slog.hpp>
#include <opencv2/opencv.hpp>
#include <librealsense2/rs.hpp>
#include <rc_message/rc_msg_server.h>
#include <rc_cv/MatConvert.h>

#define WIDTH 640
#define HEIGHT 480
#define FPS 30


namespace RC {
    namespace Task {
        void run_cv_task(rc_DeviceInfo rcDeviceInfo) {
            using namespace cv;
            slog::info << "视觉设备启动中" << slog::endl;
            rs2::context ctx;

            slog::info << "当前SDK版本 - " << RS2_API_VERSION_STR << slog::endl;
            int res_number = ctx.query_devices().size();

            slog::info << "当前深度摄像头连接数 " << res_number << " RealSense devices connected" << slog::endl;
            try {
                rs2::pipeline pipe;
                //创建一个以非默认配置的配置用来配置管道
                rs2::config cfg;
                cfg.enable_stream(RS2_STREAM_COLOR, WIDTH, HEIGHT, RS2_FORMAT_BGR8, FPS);//向配置添加所需的流
                cfg.enable_stream(RS2_STREAM_DEPTH, WIDTH, HEIGHT, RS2_FORMAT_Z16, FPS);
                pipe.start(cfg);
                try {
                    while (true) {
                        boost::mutex::scoped_lock lock(RC::Message::ImageMessage::image_mutex);
                        rs2::frameset frames = pipe.wait_for_frames();
                        rs2::depth_frame depth = frames.get_depth_frame();
                        rs2::frame color = frames.get_color_frame();

                        Mat depth_image(Size(WIDTH, HEIGHT), CV_16U, (void *) depth.get_data(), Mat::AUTO_STEP);
                        Mat color_image(Size(WIDTH, HEIGHT), CV_8UC3, (void *) color.get_data(), Mat::AUTO_STEP);
                        std::string strRespon = Mat2Base64(color_image, "bmp");
                        Message::MessageServer::imageMessage.push_message(strRespon);
//                cv::imshow("depth_image", depth_image);
//                cv::imshow("color_image", color_image);
//                if (waitKey(100) == 'q')break;

//                        float dist_to_center = depth.get_distance(WIDTH / 2, HEIGHT / 2);

//                slog::info << "The camera is facing an object " << dist_to_center << " meters away \n";
                        boost::this_thread::interruption_point();
                    }
                }catch (boost::thread_interrupted &e){
                    slog::info << "视觉任务关闭" << slog::endl;
                    boost::mutex::scoped_lock lock(RC::Message::ImageMessage::image_mutex);
                    Message::MessageServer::imageMessage.release_message();
                    pipe.stop();
                }

            } catch (rs2::error &e) {
                slog::err << "深度摄像头启动失败 " << slog::endl;
                slog::err << e.what() << slog::endl;
            }
        }
    }
}